import io
import socket
import struct
import cv2
import sys
import numpy as np
from getL import getL
from detect_laser import detect_laser
import math
import time

def client():
    params = [cv2.IMWRITE_JPEG_QUALITY, 50]

    ip_filename = "/home/pi/workspace/stereo-cv/config/ip.txt"
    ipServer = ""
    with open(ip_filename, "r") as f:  # 打开文件
        data = f.readline()  # 读取文件
        datas = data.split(" ")
        ipServer = datas[1].split("\n")[0]
        f.close()

    client_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    client_socket.connect((ipServer, 8000))

    cap = cv2.VideoCapture(0)
    height = 720
    width = 1280
    cap.set(3,width)
    cap.set(4,height)

    cnt = 0
    # 忽略前几帧
    while(cnt < 10):
        cnt += 1
        flag, frame  = cap.read()
        detect_laser(frame)

    # 获取ROI
    center = [0, 0]
    N = 10
    cnt = 0
    CNT = 0
    while(cnt < N):
        cnt += 1
        flag, frame  = cap.read()
        result = detect_laser(frame)
        if result != 0:
            CNT += 1
            center[0] += result[0]
            center[1] += result[1]
    center[0] = round(center[0] / CNT)
    center[1] = round(center[1] / CNT)
    winx = round(width / 640 * 120)
    winy = round(height / 480 * 40)
    # x1 x2 y1 y2
    roi = [center[0] - winx, center[0] + winx, center[1] - winy, center[1] + winy]

    cnt = 0
    resultList = []
    startTime = time.time()
    while cnt <= 200:
        cnt += 1
        flag, frame  = cap.read()
        frame = frame[roi[0] : roi[1], roi[2] : roi[3]]
        timestamp = time.time()

        result = detect_laser(frame)
        if(result != 0):
            result[0] += roi[0]
            result[1] += roi[2]
            result.append(timestamp)
            print(result)
            resultList.append(result)

        img_encode = cv2.imencode('.jpg', frame, params)[1]
        bytedata = img_encode.tostring()
        
        flag_data = (str(len(bytedata))).encode() + ",".encode() + " ".encode()
        client_socket.send(flag_data)
        data = client_socket.recv(1024)
        if ("ok" == data.decode()):
            client_socket.send(bytedata)
        data = client_socket.recv(1024)
        if ("ok" == data.decode()):
            pass
    print("time consume: " + str(time.time() - startTime))
    ########################################
    result_bytedata = b""
    for i in range(len(resultList)):
        result = resultList[i]
        # print(result)
        result_bytedata += (str(result[0])).encode() + ",".encode() + (str(result[1])).encode() + ",".encode() + (str(result[2])).encode() + ",".encode()

    bytedata = "result".encode() + ",".encode() + (str(len(result_bytedata))).encode()
    client_socket.send(bytedata)
    data = client_socket.recv(1024)
    if ("ok" == data.decode()):
        pass
    # 发送轨迹
    client_socket.send(result_bytedata)
    data = client_socket.recv(1024)
    if ("ok" == data.decode()):
        pass

    ########################################

    resultList = np.array(resultList)
    L = getL(resultList)
    print("L: " + str(L))

    file_handle = open('points.txt', 'w+')
    for result in resultList:
        file_handle.write("%f %f %f\n" \
            % (result[0], result[1], result[2]))
    file_handle.close()


